perm filename IMOVE2.2[EAL,HE]3 blob
sn#708945 filedate 1983-05-01 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00009 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 {$NOMAIN Small "move-related" statement interpreters }
C00028 00003 { Externally defined routines: }
C00031 00004 procedure doCmon external
C00038 00005 procedure doOperate external
C00042 00006 procedure doOpen (* & doClose *) external
C00049 00007 procedure doCenter external
C00051 00008 procedure doStop external
C00053 00009 procedure doArmmagic external
C00057 ENDMK
C⊗;
{$NOMAIN Small "move-related" statement interpreters }
const
GARMDEV = 1; (* device numbers for ARM *)
VISEDEV = 6;
SIGMAG = 20000B; (* Test only magnitude of forces *)
SIGGE = 100000B; (* Start cmon if force ≥ specified value *)
SIGLT = 0B; (* " " " " < " " *)
FSTOP = (*10000B*)4096; (* In addition to starting cmon, stop arm *)
nullingcb = 512; (* control bits for trajectory specs *)
dureqcb = 192;
destptcb = 8;
Byptcb = 256; Linearcb = 256; (* 400B *)
(* Constants from EDIT *)
maxLines = 28;
maxPPLines = 18;
maxBpts = 25;
maxTBpts = 20; (* max could be exceeded by huge case stmnt *)
listinglength = 2000; (* Length of Listingarray *)
(* Random type declarations for OMSI/SAIL compatibility *)
type
byte = 0..255; (* doesn't really belong here, but... *)
ascii = char;
atext = text;
{ Define all the pointer types here }
vectorp = ↑vector;
transp = ↑trans;
strngp = ↑strng;
eventp = ↑event;
framep = ↑frame;
statementp = ↑statement;
varidefp = ↑varidef;
nodep = ↑node;
pdbp = ↑pdb;
envheaderp = ↑envheader;
enventryp = ↑enventry;
environp = ↑environment;
cmoncbp = ↑cmoncb;
messagep = ↑message;
(* This one is used whenever a pointer is needed for which the *)
(* definition is missing from this file; naturally, all *)
(* pointers use the same space *)
dump = ↑integer;
token = array[1..4] of integer; {Uses same space as a token}
cursorp = array[1..4] of integer; {Ditto, for cursorp}
(* datatype definitions *)
datatypes = (pconstype, varitype, svaltype, vectype, rottype, transtype,
frametype, eventtype, strngtype, labeltype, proctype, arraytype,
reftype, valtype, cmontype, nulltype, undeftype,
dimensiontype, mactype, macargtype, freevartype);
scalar = real;
u = (used,free);
vector = record case u of
used: (refcnt: integer; val: array [1..3] of real);
free: (next: vectorp);
end;
trans = record case u of
used: (refcnt: integer; val: array [1..3,1..4] of real);
free: (next: transp);
end;
cstring = packed array [1..10] of ascii;
c4str = packed array [1..4] of ascii;
c5str = packed array [1..5] of ascii;
c20str = packed array [1..20] of ascii;
linestr = packed array [1..130] of ascii;
strng = record
next: strngp;
ch: cstring;
end;
event = record
next: eventp; (* all events are on one big list *)
count: integer;
waitlist: pdbp;
end;
frame = record
vari: varidefp; (* back pointer to variable name & info *)
calcs: nodep; (* affixment info *)
case ftype: boolean of (* frame = true, device = false *)
true: (valid: integer; val, fdepr: transp; dcntr: integer; dev: framep);
false: (mech: integer; case sdev: boolean of
true: (sdest: real); false: (tdest,appr,depr: transp));
(* sdev = true for scalar devices, false for frames *)
end;
(* statement definitions *)
stmntypes = (progtype, blocktype, coblocktype, endtype, coendtype,
fortype, iftype, whiletype, untiltype, casetype,
calltype, returntype,
printtype, prompttype, pausetype, aborttype, assigntype,
signaltype, waittype, enabletype, disabletype, cmtype,
affixtype, unfixtype,
movetype,jtmovetype,operatetype,opentype,closetype,centertype,
floattype, stoptype, retrytype,
requiretype, definetype, macrotype, commenttype, dimdeftype,
setbasetype, wristtype, saytype, declaretype, emptytype,
evaltype, armmagictype);
(* more??? *)
statement = packed record
next, last: statementp;
stlab: varidefp;
exprs: nodep; (* any expressions used by this statement *)
nlines: integer;
bpt,bad: boolean;
case stype: stmntypes of
coendtype: (bcode, bparent: statementp; blkid: dump;
level, numvars: 0..255; variables: varidefp);
movetype,
jtmovetype,
operatetype,
opentype,
closetype,
centertype,
floattype,
setbasetype,
stoptype: (cf, clauses: nodep);
armmagictype: (cmdnum,dev,iargs,oargs: nodep);
cmtype: (oncond: nodep; conclusion: statementp;
deferCm, exprCm: boolean; cdef: varidefp);
end;
(* auxiliary definitions: variable, etc. *)
varidef = packed record
next,dnext: varidefp;
name: dump;
level: 0..255; (* environment level *)
offset: 0..255; (* environment offset *)
dtype: varidefp; (* to hold the dimension info *)
tbits: 0..15; (* special type bits: array = 1, proc = 2, ref = 4 & ? *)
dbits: 0..15; (* for use by debugger/interpreter *)
case vtype: datatypes of
arraytype: (a: nodep);
proctype: (p: nodep);
labeltype,
cmontype: (s: statementp);
mactype: (mdef: statementp);
macargtype: (marg: dump);
pconstype: (c: nodep);
dimensiontype: (dim: nodep);
end;
(* definition of the ubiquitous NODE record *)
nodetypes = (exprnode, leafnode, listnode, clistnode, colistnode, forvalnode,
deprnode, viaptnode, apprnode, destnode, byptnode, durnode,
sfacnode, wobblenode, swtnode, nullingnode, wristnode, cwnode,
arrivalnode, departingnode,
ffnode, forcenode, stiffnode, gathernode, cmonnode, errornode,
calcnode, arraydefnode, bnddefnode, bndvalnode,
waitlistnode, procdefnode, tlistnode, dimnode, commentnode,
linearnode, elbownode, shouldernode, flipnode, wrtnode,
loadnode,velocitynode);
exprtypes = ( svalop, (* scalar operators *)
sltop, sleop, seqop, sgeop, sgtop, sneop, (* relations *)
notop, orop, xorop, andop, eqvop, (* logical *)
saddop, ssubop, smulop, sdivop, snegop, sabsop, (* scalar ops *)
sexpop, maxop, minop, intop, idivop, modop,
sqrtop, logop, expop, timeop, (* functions *)
sinop, cosop, tanop, asinop, acosop, atan2op, (* trig *)
vdotop, vmagnop, tmagnop,
vecop, (* vector operators *)
vmakeop, unitvop, vaddop, vsubop, crossvop, vnegop,
svmulop, vsmulop, vsdivop, tvmulop, wrtop,
tposop, taxisop,
transop, (* trans operators *)
tmakeop, torientop, ttmulop, tvaddop, tvsubop, tinvrtop,
vsaxwrop, constrop, ftofop, deproachop, fmakeop, vmkfrcop,
ioop, (* i/o operators *)
queryop, inscalarop,
specop, (* special operators *)
arefop, callop, grinchop, macroop, vmop, adcop, dacop, jointop,
badop,
addop, subop, negop, mulop, divop, absop); (* for parsing *)
leaftypes = pconstype..strngtype;
reltypes = sltop..sgtop;
forcetypes = (force,absforce,torque,abstorque,angvelocity);
node = record
next: nodep;
case ntype: nodetypes of
exprnode: (op: exprtypes; arg1, arg2, arg3: nodep; elength: integer);
leafnode: (case ltype: leaftypes of
varitype: (vari: varidefp; vid: dump);
pconstype: (cname: varidefp; pcval: nodep);
svaltype: (s: scalar; wid: integer);
vectype: (v: vectorp);
transtype: (t: transp);
strngtype: (length: integer; str: strngp) ); (* also used by commentnodes *)
arrivalnode:(evar: varidefp);
wrtnode,
deprnode,
apprnode,
destnode: (loc: nodep; code: statementp);
byptnode,
viaptnode: (vlist: boolean; via,vclauses: nodep; vcode: statementp);
durnode: (durrel: reltypes; durval: nodep);
velocitynode,
sfacnode,
wobblenode,
swtnode: (clval: nodep);
nullingnode, (* true = nonulling *)
wristnode, (* = don't zero force wrist *)
cwnode, (* = counter_clockwise *)
elbownode, (* = elbow up *)
shouldernode, (* = right shoulder *)
flipnode, (* = don't flip wrist *)
linearnode: (notp: boolean); (* = linear motion *)
ffnode: (ff,cf: nodep; csys, pdef: boolean); (* true = world, false = hand *)
loadnode: (loadval,loadvec: nodep; lcsys: boolean); (* lcsys = csys above *)
forcenode: (ftype: forcetypes; frel: reltypes; fval, fvec, fframe: nodep);
stiffnode: (fv, mv, cocff: nodep);
gathernode: (gbits: integer);
cmonnode: (cmon: statementp; errhandlerp: boolean);
errornode: (eexpr: nodep);
end;
(* process descriptor blocks & environment record definitions *)
queuetypes = (nullqueue,nowrunning,runqueue,inputqueue,eventqueue,sleepqueue,
forcewait,devicewait,joinwait,proccall);
pdb = packed record
nextpdb,next: pdbp; (* for list of all/active pdb's *)
level: 0..255; (* lexical level *)
mode: 0..255; (* expression/statement/sub-statement *)
priority: 0..255; (* probably never greater than 3? *)
status: queuetypes; (* what are we doing *)
env: envheaderp;
spc: statementp; (* current statement *)
epc: nodep; (* current expression (if any) *)
sp: nodep; (* intermediate value stack *)
cm: cmoncbp; (* if we're a cmon point to our definition *)
mech: framep; (* current device being used *)
linenum: integer; (* used by editor/debugger *)
case procp: boolean of (* true if we're a procedure *)
true: (opdb: pdbp; (* pdb to restore when procedure exits *)
pdef: nodep); (* procedure definition node *)
false: (evt: eventp; (* event to signal when process goes away *)
sdef: statementp); (* first statement where process was defined *)
end;
envheader = packed record
parent: envheaderp;
env: array [0..4] of environp;
varcnt: 0..255; (* # of variables in use ??? *)
case procp: boolean of (* true if we're a procedure *)
true: (proc: nodep);
false:(block: statementp);
end;
enventry = record
case etype: datatypes of
svaltype: (s: scalar);
vectype: (v: vectorp);
transtype: (t: transp);
frametype: (f: framep);
eventtype: (evt: eventp);
strngtype: (length: integer; str: strngp);
cmontype: (c: cmoncbp);
proctype: (p: nodep; penv: envheaderp);
reftype: (r: enventryp);
arraytype: (a: envheaderp; bnds: nodep);
end;
environment = record
next: environp;
vals: array [0..9] of enventryp;
end;
cmoncb = record
running, enabled: boolean; (* cmon's status *)
cmon: statementp;
pdb: pdbp;
evt: eventp;
fbits: integer; (* bits for force sensing *)
oldcmon: cmoncbp; (* for debugger *)
end;
(* definition of AL-ARM messages *)
msgtypes = (initarmscmd,calibcmd,killarmscmd,wherecmd,
abortcmd,stopcmd,movehdrcmd,movesegcmd,
centercmd,operatecmd,movedonecmd,signalcmd,
setccmd,forcesigcmd,forceoffcmd,biasoncmd,biasoffcmd,setstiffcmd,
zerowristcmd,wristcmd,gathercmd,getgathercmd,readadccmd,writedaccmd,
errorcmd,floatcmd,setloadcmd,
armmagiccmd,realcmd,vectorcmd,transcmd);
errortypes = (noerror,noarmsol,timerr,durerr,toolong,featna,
unkmess,srvdead,adcdead,nozind,exjtfc,paslim,nopower,badpot,devbusy,
baddev,timout,panicb,nocart,cbound,badparm);
message = record
cmd: msgtypes;
ok: boolean;
case integer of
1: (dev, bits, n: integer;
(* (dev, bits, n, evt: integer; (* for arm code version *)
evt: eventp;
dur: real;
case integer of
1: (v1,v2,v3: real);
2: (sfac,wobble,pos: real);
3: (val,angle,mag: real);
4: (max,min: real);
5: (error: errortypes));
2: (fv1,fv2,fv3,mv1,mv2,mv3: real); (* may never use these... *)
3: (t: array [1..6] of real);
end;
interr = record
case integer of
0: (i: integer);
1: (err,foo: errortypes);
end;
listingarray = packed array [0..listinglength] of ascii;
(* global variables *)
var
(* from EDIT *)
listing: listingarray; (* first 150 chars are used by expression editor *)
(* next 40 by header & trailer lines *)
{*} cursorStack: array [1..15] of cursorp; {These are BIG records! }
(* lbuf: array [1..160] of ascii;
ppBuf: array [1..100] of ascii; *)
dum1: array[1..260] of ascii;
lines: array [1..maxLines] of dump;
ppLines: array [1..maxPPLines] of dump;
(* marks: array [1..20] of integer;
reswords: array [0..26] of reswordp;
idents: array [0..26] of identp;
macrostack: array [1..10] of tokenp;
curmacstack: array [1..10] of varidefp;
screenheight,dispHeight: integer;
ppBufp,oppBufp,ppOffset,ppSize,nmarks: integer;
lbufp,cursor,ocur,cursorLine,fieldnum,lineNum,findLine,pcLine: integer;
firstDline,topDline,botDline,firstLine,lastLine,curLine: integer;
freeLines,oldLines: linerecp;
sysVars: varidefp;
dProg: statementp;
curBlock, newDeclarations, findStmnt: statementp;
macrodepth: integer;
filedepth, errCount, sCursor: integer;
curChar, maxChar, curFLine, curPage: integer;
nodim, distancedim, timedim, angledim,
forcedim, torquedim, veldim, angveldim: varidefp;
fvstiffdim, mvstiffdim: nodep;
pnode: nodep;
*) dum2: array[1..141] of dump;
(* smartTerminal: boolean;
setUp,setExpr,setCursor,dontPrint,outFilep,collect,fParse,sParse,
eofError,endOfLine,backup,expandmacros,flushcomments,checkDims,
shownLine: boolean;
*) dum3: array[1..16] of boolean;
curtoken: token;
file1,file2,file3,file4,file5,outFile: atext;
bpts: array [1..maxBpts] of statementp; (* debugging crap *)
tbpts: array [1..maxTBpts] of statementp;
debugPdbs: array [0..10] of pdbp;
(* nbpts,ntbpts,debugLevel: integer;
eCurInt: pdbp;
STLevel: integer;
*) dum4: array[1..5] of integer;
singleThreadMode,tSingleThreadMode: boolean;
(* from INTERP *)
inputLine: array [1..20] of ascii;
talk: text; (* for using the speech synthesizer *)
curInt, activeInts, readQueue, allPdbs: pdbp;
sysEnv: envheaderp;
clkQueue: nodep;
allEvents: eventp;
etime: integer; (* used by eval *)
curtime: integer; (* who knows where this will get updated - an ast? *)
stime: integer; (* used for clock queue on 10 *)
msg: messagep; (* for AL-ARM interaction *)
inputp: integer; (* current offset into inputLine array above *)
resched, running, escapeI, iSingleThreadMode: boolean;
msgp: boolean; (* flag set if any messages pending *)
inputReady: boolean;
(* various constant pointers *)
xhat,yhat,zhat,nilvect: vectorp;
niltrans: transp;
gpark, rpark: transp; (* arm park positions *)
(* various device & variable pointers *)
speedfactor: enventryp;
garm: framep;
{ Externally defined routines: }
(* From ALLOC *)
procedure relNode(n: nodep); external;
(* From IAUX1A *)
function pop: nodep; external;
function getEntry (level, offset: byte): enventryp; external;
function getVar (level, offset: byte): enventryp; external;
function gtVarn (n: nodep): enventryp; external;
function getNval(n: nodep; var b: boolean): nodep; external;
function getEvent: eventp; external;
procedure freeEvent(e: eventp); external;
procedure sendCmd; external;
procedure killNode(n: nodep); external;
(* From IAUX1B *)
procedure addPdb(var plist: pdbp; pn: pdbp); external;
procedure sleep(whenV: integer); external;
(* From IAUX2B *)
procedure cmonEnable(e: enventryp); external;
(* From IROOT *)
function m2Forcebits(fn: nodep; var negv: boolean): integer; external;
function m2GetMechbits: integer; external;
procedure m2MvStart; external;
procedure m2MvEnd; external;
procedure m2MvRetry; external;
(* From RSXMSG *)
procedure signalArm; external;
(* Display-related Routines *)
procedure ppLine; external;
procedure ppOutNow; external;
procedure ppChar(ch: ascii); external;
procedure pp5(ch: c5str; length: integer); external;
procedure pp10(ch: cstring; length: integer); external;
procedure pp10L(ch: cstring; length: integer); external;
procedure pp20(ch: c20str; length: integer); external;
procedure pp20L(ch: c20str; length: integer); external;
procedure ppInt(i: integer); external;
procedure ppReal(r: real); external;
procedure ppStrng(length: integer; s: strngp); external;
procedure iMove2Get; external;
procedure iMove2Get; begin end;
procedure doCmon; external;
procedure doCmon;
var e: enventryp; n: nodep; b: boolean; val: nodep; r: real; fbits,i: integer;
sst: statementp;
begin
with curInt↑ do
case mode of
1: begin
if not spc↑.deferCm then (* check it's not a deferred cmon *)
begin (* need to enable the cmon *)
with spc↑.cdef↑ do
cmonEnable(getEntry(level,offset)); (* enable cmon control block *)
end;
mode := 0;
spc := spc↑.next;
end;
2: begin (* deal with ON condition *)
n := nil;
mode := 3; (* set up for doing conclusion next time *)
if spc↑.exprCm then
begin (* test if expression is now true *)
n := pop; (* get expression value *)
if n↑.s = 0.0 then
begin
sleep(20); (* no good - try again in 0.33 seconds *)
mode := 0;
end;
end
else if spc↑.oncond↑.ntype = durnode then
begin (* duration cmon *)
n := pop;
sleep(round(n↑.s * 60)); (* get wait time (in 60Hz ticks) *)
end
else if spc↑.oncond↑.ntype = forcenode then
begin (* force sensing *)
val := getNval(spc↑.oncond↑.fval,b); (* get force magnitude *)
r := val↑.s;
if b then relNode(val);
fbits := m2Forcebits(spc↑.oncond,b);
with spc↑.oncond↑ do
begin
if (ftype = absforce) or (ftype = abstorque) then fbits := fbits + SIGMAG;
if b then begin r := -r; if frel < seqop then fbits := fbits + SIGGE end
else if frel >= seqop then fbits := fbits + SIGGE;
end;
with spc↑.conclusion↑ do
if stype = stoptype then
begin (* set FSTOP bit if no explicit frame is being stopped *)
if cf = nil then fbits := fbits + FSTOP
else if cf↑.ntype = leafnode then
begin (* need to check if same device as current mech *)
e := gtVarn(cf); (* get variable frame *)
if e↑.etype = frametype then
begin
if e↑.f = nil then i := GARMDEV (* default to green arm *)
else with e↑.f↑ do
if ftype then
if dev <> nil then i := dev↑.mech
else i := GARMDEV (* default to green arm *)
else i := mech;
if i = m2GetMechBits then fbits := fbits + FSTOP;
end
end;
(* ** can't check if array ref since subscripts aren't on stack ** *)
end
else if stype = blocktype then
if bcode↑.stype = stoptype then
if bcode↑.cf = nil then fbits := fbits + FSTOP;
cm↑.fbits := fbits; (* remember bits in cmoncb *)
with msg↑ do
begin
cmd := forcesigcmd;
dev := m2GetMechbits; (* deal with which arm here *)
bits := fbits;
evt := cm↑.evt;
mag := r;
end;
sendCmd;
cm↑.evt↑.count := -1;
cm↑.evt↑.waitlist := curInt; (* put us on event waitlist *)
curInt↑.status := forcewait;
curInt := nil; (* swap in someone else *)
resched := true;
end
else if spc↑.oncond↑.ntype = departingnode then
begin (* departing cmon *)
sleep(30); (* wait 0.5 seconds (in 60Hz ticks) *)
end
else
begin (* event cmon *)
if spc↑.oncond↑.ntype = arrivalnode then
with spc↑.oncond↑.evar↑ do e := getVar(level,offset)
else e := gtVarn(spc↑.oncond);
cm↑.evt := e↑.evt; (* save pointer to event we're waiting on *)
e↑.evt↑.count := e↑.evt↑.count - 1;
if e↑.evt↑.count <= 0 then (* hasn't been signalled yet, need to wait *)
begin
addPdb(e↑.evt↑.waitlist,curInt); (* add us to wait list *)
curInt↑.status := eventqueue;
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
if n <> nil then relNode(n);
end;
3: begin
mode := 0;
if cm↑.enabled then (* check that we're still enabled *)
begin
cm↑.running := true; (* set up current cmon status *)
cm↑.enabled := false;
spc := spc↑.conclusion;
end
else
begin
curInt↑.status := nullqueue;
curInt := nil; (* we should go away *)
resched := true; (* now swap in highest priority process *)
end;
end;
end;
end;
procedure doOperate; external;
procedure doOperate;
var durcl,vel,torquecl,cl,v: nodep; e: enventryp; b,ccw: boolean; ev: eventp;
begin (* deal with driver *)
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
m2MvStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
durcl := nil;
vel := nil;
torquecl := nil;
ccw := false;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
if ntype = durnode then durcl := cl
else if ntype = forcenode then
begin
if ftype = torque then torquecl := cl
else if ftype = angvelocity then vel := cl
end
else if ntype = cwnode then ccw := notp;
cl := next;
end;
with msg↑ do
begin
cmd := operatecmd;
dev := m2GetMechbits;
bits := 0;
evt := ev;
dur := 5.0; (* default values *)
v1 := 60.0; (* angular velocity *)
v2 := 0.0; (* torque *)
if durcl <> nil then
begin
v := getNval(durcl↑.durval,b); (* get duration value *)
dur := v↑.s;
if b then relNode(v);
end;
if vel <> nil then
begin
v := getNval(vel↑.fval,b); (* get angular velocity value *)
v1 := v↑.s;
if b then relNode(v);
end;
if torquecl <> nil then
begin
v := getNval(torquecl↑.fval,b); (* get torque value *)
v2 := v↑.s;
if b then relNode(v);
end;
if ccw then
begin (* turning counterclockwise *)
v1 := - v1;
v2 := - v2;
end;
end;
sendCmd; (* pass info to ARM servo *)
mode := 3;
curInt↑.status := devicewait; (* don't for simulation version *)
curInt := nil;
resched := true; (* swap someone else in *)
end;
3: m2MvEnd; (* do end of motion cleanup, run error handler, etc. *)
4: m2MvRetry; (* deal with user response if there was an error *)
end;
end;
procedure doOpen; (* & doClose *) external;
procedure doOpen;
var dest,bydest,sfac,durcl,swt,cl,v: nodep; e: enventryp; ev: eventp;
opening,dtime,sf,swtime: real; mechbits: integer; b,nulling: boolean;
begin
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
m2MvStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := m2GetMechbits;
(* run through clauses for dest, duration & speed factor/stop wait time specs *)
dest := nil;
bydest := nil;
durcl := nil;
sfac := nil;
swt := nil;
nulling := true; (* nonulling is the default *)
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
case ntype of
destnode: dest := cl;
byptnode: bydest := cl;
durnode: durcl := cl;
sfacnode: sfac := cl;
swtnode: swt := cl;
nullingnode: nulling := notp;
otherwise begin (* nothing to do *) end;
end;
cl := next;
end;
if sfac = nil then sf := speedfactor↑.s (* use global speed factor *)
else
begin
v := getNval(sfac↑.clval,b); (* get local speed factor value *)
sf := v↑.s;
if b then relNode(v);
end;
if durcl = nil then dtime := 0
else
begin
v := getNval(durcl↑.durval,b); (* get duration value *)
dtime := v↑.s;
if b then relNode(v);
end;
if swt = nil then swtime := 0
else
begin
v := getNval(swt↑.clval,b); (* get stop wait time value *)
swtime := v↑.s;
if b then relNode(v);
end;
if dest <> nil then
begin
v := getNval(dest↑.loc,b); (* get opening value *)
opening := v↑.s;
mech↑.sdest := opening; (* remember it *)
if b then relNode(v);
end
else if bydest <> nil then
begin
v := getNval(bydest↑.loc,b); (* get opening value *)
opening := v↑.s;
mech↑.sdest := mech↑.sdest + opening; (* remember it *)
if b then relNode(v);
end
else
begin
opening := 0;
mech↑.sdest := -1; (* so we know there was no dest *)
end;
with msg↑ do
begin
dev := mechbits;
evt := ev;
if nulling then bits := NULLINGCB else bits := 0;
if (dest <> nil) or (bydest <> nil) then
begin
pos := opening;
bits := bits + DESTPTCB; (* indicate we're specifying opening *)
if dest = nil then bits := bits + BYPTCB; (* tell ARM incremental motion *)
end
else
begin
pos := 0.0;
if spc↑.stype = opentype then bits := 3 else bits := 1;
end;
if durcl = nil then dur := 0.0
else
begin
dur := dtime;
bits := bits + DUREQCB;
end;
sfac := sf;
if mechbits = VISEDEV then
begin
cmd := operatecmd; (* vise uses an operate command *)
if swt = nil then
if dest = nil then v2 := 0.25 else v2 := 0.0 (* default values *)
else v2 := swtime;
if durcl = nil then dur := 8.0;
sendCmd;
end
else
begin
cmd := movehdrcmd; (* deal with hand *)
sendCmd;
signalArm; (* since movehdr normally followed by movesegs *)
end;
end;
mode := 3;
curInt↑.status := devicewait; (* don't for simulation version *)
curInt := nil;
resched := true; (* swap someone else in *)
end;
3: m2MvEnd; (* do end of motion cleanup, run error handler, etc. *)
4: m2MvRetry; (* deal with user response if there was an error *)
end;
end;
procedure doCenter; external;
procedure doCenter;
var e: enventryp; ev: eventp;
begin
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
m2MvStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
with msg↑ do
begin
cmd := centercmd;
dev := m2GetMechbits;
bits := 0;
evt := ev;
end;
sendCmd; (* initiate the center operation *)
mode := 3;
curInt↑.status := devicewait; (* don't for simulation version *)
curInt := nil;
resched := true; (* swap someone else in *)
end;
3: m2MvEnd; (* do end of motion cleanup, run error handler, etc. *)
4: m2MvRetry; (* deal with user response if there was an error *)
end;
end;
procedure doStop; external;
procedure doStop;
var mechbits: integer; e: enventryp;
procedure complain;
begin (* yow! frame that's not affixed to a device *)
pp20L('Attempt to stop fram',20); pp20('e not affixed to any',20);
pp20(' device: Assuming ga',20); pp5('rm ',2); ppLine;
mechbits := GARMDEV;
end;
begin
with curInt↑ do
begin
if spc↑.cf = nil then
if mech = nil then complain
else mechbits := m2GetMechbits (* use current mech *)
else
begin
e := gtVarn(spc↑.cf); (* see what we're stopping *)
with e↑.f↑ do
if ftype then
if dev <> nil then mechbits := dev↑.mech
else complain
else mechbits := mech;
end;
with msg↑ do
begin
cmd := stopcmd;
dev := mechbits;
end;
sendCmd; (* tell arm servo to stop device *)
mode := 0;
spc := spc↑.next;
end;
end;
procedure doArmmagic; external;
procedure doArmmagic;
var e: enventryp; ev: eventp; np: nodep; i,j,k: integer;
begin
with curInt↑ do
case mode of
1: begin
np := pop;
i := round(np↑.s); (* get # of arm magic command *)
relNode(np);
e := gtVarn(spc↑.dev); (* remember what we're moving *)
mech := e↑.f;
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
j := 0;
np := spc↑.iargs;
while np <> nil do begin np := np↑.next; j := j + 1 end; (* count args *)
with msg↑ do
begin
cmd := armmagiccmd;
n := i; (* command number *)
dev := m2GetMechbits;
bits := j;
evt := ev;
sendCmd; (* initiate the armmagic operation *)
for i := 1 to j do
begin (* send over the arguments *)
np := pop; (* get next argument *)
if np↑.ltype = svaltype then
begin
cmd := realcmd;
dur := np↑.s
end
else if np↑.ltype = vectype then
begin
cmd := vectorcmd;
with np↑.v↑ do
begin
v1 := val[1]; (* copy vector *)
v2 := val[2];
v3 := val[3];
end
end
else if np↑.ltype = transtype then
begin
cmd := transcmd;
with np↑.t↑ do
begin
for k := 1 to 3 do begin t[k] := val[k,1]; t[k+3] := val[k,2] end;
sendCmd; (* send first packet of trans over *)
for k := 1 to 3 do begin t[k] := val[k,3]; t[k+3] := val[k,4] end;
end;
end
else
begin (* error -- must be string type *)
pp20L('ARM MAGIC can''t hand',20); pp10('le strings',10); ppLine;
cmd := realcmd;
dur := 0.0; (* send a zero instead *)
end;
sendCmd; (* send real/vector/2nd-half-of-trans over *)
killNode(np); (* flush used stack entry *)
end;
end;
signalArm; (* start things happening *)
mode := 2;
status := devicewait;
curInt := nil;
resched := true; (* swap someone else in *)
end;
2: begin
mode := 0; (* get ready for next statement *)
spc := spc↑.next;
end
end;
end;